#include "stdafx.h"
#include "IntrinsicLS.h"
#include "cvxSf.h"
#include <map>
#include <vector>
#include "least_square.h"



using cvx::Bw8UImage;
using cvx::Rgb8UImage;
using cvx::Bw32FImage;
using cvx::Bw32SImage;
using namespace std;

void solve_intrinsic_ls(const Mat &orgImg, const Mat &shaAbsMap, 
						const Mat &simShaMap, const Mat &simRefMap,						
						const vector<int> &simRefPixNum, const vector<int> &simShaPixNum)
{
//	cout<<orgImg.depth()<<endl;
	assert(orgImg.depth() == CV_8U);
	assert(orgImg.channels() == 3);
	const int w = orgImg.cols;
	const int h = orgImg.rows;

	//define constant parameters
	const int WSIZE = 3;    //size of the local windows
	const int WSIZE2 = 9;   //WSIZE2 = WSIZE*WSIZE, number of pixels in each window ('n' in the paper)
	const int MSIZE = 12;   //WSIZE2+3, number of rows in the M matrix (see equation 8)
	const int WSIZE_2 = 1;  //WSIZE_2 = WSIZE/2
	const int SIZE_N = 5;   //2*2*WSIZE_2+1, size of the total neighborhood affected by a pixel
	const int SIZE_N2 = 25; //SIZE_N*SIZE_N, number of non-zero element per-pixel in the L matrix 
	double epsilon = 0.000001;
	double sqrt_epsilon = sqrt(epsilon);

	//result Mat
	//unknown index map
	Mat rstMat   = Mat::zeros(h, w, CV_32F);
	Mat unIdxMat = Mat::zeros(h, w, CV_32S);
	
	Bw32SImage idx_map(&unIdxMat);
	Bw8UImage  abs_map(&shaAbsMap);
	Bw32FImage rst_mat(&rstMat);

	//set shading absolute area and unknown index area
	int unKnowNum = 0;
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			if (abs_map[y][x] != 0)
			{
				idx_map[y][x] = -1;
				rst_mat[y][x] = 1.0;
			}
			else
			{
				idx_map[y][x] = unKnowNum;
				unKnowNum++;
			}
		}
	}

	/* Set M */
	double M[MSIZE][3]; //M matrix (see equation 8)
	M[MSIZE-3][0] = sqrt_epsilon; M[MSIZE-3][1] = 0; M[MSIZE-3][2] = 0;
	M[MSIZE-2][0] = 0; M[MSIZE-2][1] = sqrt_epsilon; M[MSIZE-2][2] = 0;
	M[MSIZE-1][0] = 0; M[MSIZE-1][1] = 0; M[MSIZE-1][2] = sqrt_epsilon;

	vector<map<int, double>> matrix_vec;
	vector<double> right_vec;

	//equation 10 NiSi = 0
	Rgb8UImage org_img(&orgImg);
	for (int y = 1; y<h-1; ++y)
	{
		for (int x = 1; x<w-1; ++x)
		{
			//fill the M matrix with the image values in the current window
			//Equ 8
			for (int m=-WSIZE_2; m<=WSIZE_2; m++) 
			{
				int yC = y+m;
				for (int n=-WSIZE_2; n<=WSIZE_2; n++)
				{
					int xC = x+n;
					M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][0] = org_img[yC][xC].r;
					M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][1] = org_img[yC][xC].g;
					M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][2] = org_img[yC][xC].b;
				}
			}
			//get Ni
			Mat Mi  = Mat(MSIZE,3,CV_64F, M);
			Mat Mit = Mi.t();
			Mat MitMi = Mit * Mi;
			Mat MitMi_inv = MitMi.inv();
			Mat Ni = Mi * MitMi_inv * Mit;
			Ni = Mat::eye(Ni.rows, Ni.cols, CV_64F) - Ni;			

			double *pNi = (double*)(Ni.data);
			for (int j = 0; j<Ni.rows; ++j)
			{
				map<int, double> imap;
				double rightVal = 0.0;
				for (int k = 0; k<Ni.cols-3; ++k)
				{
					int niIdx = j * Ni.cols + k;

					//sIdx position in original map
					int sy = k/WSIZE - WSIZE_2 + y;
					int sx = k%WSIZE - WSIZE_2 + x;
					if (idx_map[sy][sx] == -1)
					{
						rightVal -= pNi[niIdx] * rst_mat[sy][sx];
					}
					else
					{						
						int sIdx = idx_map[sy][sx];
						imap[sIdx] = pNi[niIdx];	
					}
								
				}
				matrix_vec.push_back(imap);
				right_vec.push_back(rightVal);
			}
		}
	}

	//Equ 13 constant reflectance
	Bw8UImage ref_map(&simRefMap);
	for (int y = 0; y<h-1; ++y)
	{
		for (int x = 0; x<w-1; ++x)
		{
			int unIdx = idx_map[y][x];
			//constant reflectance and un-determined illumination
			if (ref_map[y][x] != 0 && idx_map[y][x] != -1)
			{
				float wt = 1.0/simRefPixNum[ref_map[y][x]];
			//	float wt = 1.0;
#if 1
				//right
				if (ref_map[y][x] == ref_map[y][x+1] && idx_map[y][x+1] != -1)
				{
					int rIdx = idx_map[y][x+1];
					map<int, double> imap;
					double rightVal = 0.0;

					imap[unIdx] =  wt * org_img[y][x+1].r;
					imap[rIdx]  = -wt * org_img[y][x].r;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
					imap.clear();

					imap[unIdx] =  wt * org_img[y][x+1].g;
					imap[rIdx]  = -wt * org_img[y][x].g;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
					imap.clear();

					imap[unIdx] =  wt * org_img[y][x+1].b;
					imap[rIdx]  = -wt * org_img[y][x].b;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
					imap.clear();
				}

				//down
				if (ref_map[y][x] == ref_map[y+1][x] && idx_map[y+1][x] != -1)
				{
					int dIdx = idx_map[y+1][x];

					map<int, double> imap;
					double rightVal = 0.0;

					imap[unIdx] =  wt * org_img[y+1][x].r;
					imap[dIdx]  = -wt * org_img[y][x].r;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
					imap.clear();

					imap[unIdx] =  wt * org_img[y+1][x].g;
					imap[dIdx]  = -wt * org_img[y][x].g;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
					imap.clear();

					imap[unIdx] =  wt * org_img[y+1][x].b;
					imap[dIdx]  = -wt * org_img[y][x].b;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
					imap.clear();
				}

				//right down
				if (ref_map[y][x] == ref_map[y+1][x+1] && idx_map[y+1][x+1] != -1)
				{
					int rdId = idx_map[y+1][x+1];

					map<int, double> imap;
					double rightVal = 0.0;

					imap[unIdx] =  wt * org_img[y+1][x+1].r;
					imap[rdId]  = -wt * org_img[y][x].r;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
					imap.clear();

					imap[unIdx] =  wt * org_img[y+1][x+1].g;
					imap[rdId]  = -wt * org_img[y][x].g;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
					imap.clear();

					imap[unIdx] =  wt * org_img[y+1][x+1].b;
					imap[rdId]  = -wt * org_img[y][x].b;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);					
				}
#endif
			}			

		}
	}

	//Equ 15
	Bw8UImage sha_sim(&simShaMap);
	for (int y = 0; y<h-1; ++y)
	{
		for (int x = 0; x<w-1; ++x)
		{
			if (sha_sim[y][x] != 0 && idx_map[y][x] != -1)
			{
				float wt = 1.0/simShaPixNum[sha_sim[y][x]];
				int unIdx = idx_map[y][x];
				//right
				if (sha_sim[y][x] == sha_sim[y][x+1] && idx_map[y][x+1] != -1)
				{
					int rIdx = idx_map[y][x+1];

					map<int, double> imap;
					double rightVal = 0.0;

					imap[unIdx] =  wt * 1;
					imap[rIdx]  = -wt * 1;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
				}

				//down
				if (sha_sim[y][x] == sha_sim[y+1][x] && idx_map[y+1][x] != -1)
				{
					int dIdx = idx_map[y+1][x];

					map<int, double> imap;
					double rightVal = 0.0;

					imap[unIdx] =  wt * 1;
					imap[dIdx]  = -wt * 1;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
				}

				//right down
				if (sha_sim[y][x] == sha_sim[y+1][x+1] && idx_map[y+1][x+1] != -1)
				{
					int rdIdx = idx_map[y+1][x+1];

					map<int, double> imap;
					double rightVal = 0.0;

					imap[unIdx] =  wt * 1;
					imap[rdIdx] = -wt * 1;
					matrix_vec.push_back(imap);
					right_vec.push_back(rightVal);
				}
			}
		}
	}
	

	if (matrix_vec.size() != right_vec.size())
	{
		return;
	}
	else
	{
		cout<<"complete equation"<<endl;
	}

	cout<<"matris_vec size = "<<matrix_vec.size()<<endl;
	
	vector<double> result(unKnowNum);
	//solve AX = b, b != 0
//	bool isOk = CvxSF::LeastSquare(matrix_vec, right_vec, result);
	bool isOk = LeastSquare::solver(matrix_vec, right_vec, true, unKnowNum, &result[0]);
	if (isOk)
	{
		for (int y = 0; y<h; ++y)
		{
			for (int x = 0; x<w; ++x)
			{
				if (idx_map[y][x] != -1)
				{
					rst_mat[y][x] = result[idx_map[y][x]];					
				}				
			}
		}

		Mat reflectanceMat(rstMat.rows, rstMat.cols, CV_8UC3);

		Rgb8UImage ref_mat(&reflectanceMat);
		for (int y = 0; y<h; ++y)
		{
			for (int x =0 ; x<w; ++x)
			{
				float sVal = rst_mat[y][x];
				if (sVal != 0.0)
				{
					ref_mat[y][x].r = org_img[y][x].r /sVal;
					ref_mat[y][x].g = org_img[y][x].g /sVal;
					ref_mat[y][x].b = org_img[y][x].b /sVal;
				}
			}
		}	

		imshow("reflectance image", reflectanceMat);
		imwrite("flower_ref.png", reflectanceMat);


		rstMat.convertTo(rstMat, CV_8U, 255.0);
		imshow("result", rstMat);
		imwrite("flower_LS.png", rstMat);

		
	}
	else
	{
		cout<<"least square failed"<<endl;
	}
}

void test_4DlinearModel(const Mat &orgImage, const Mat &shadingGT)
{
	//Si = a1*Ir + a2*Ig + a3*Ib
	vector<map<int, double>> left_vec;
	vector<double> right_vec;

	int w = orgImage.cols, h = orgImage.rows;
	Rgb8UImage org_img(&orgImage);
	Bw8UImage  sha_img(&shadingGT);

//	const int WINDOW_SIZE = 2;
	double epsilon  = 0.0001;
	double epsilon2 = sqrt(epsilon); 
	int unIdx = 0;
	for (int y = 1; y<h-1; ++y)
	{
		for (int x = 1; x<w-1; ++x)
		{
			//check if all data is the same
			int r, g, b, s;
			r = org_img[y][x].r;
			g = org_img[y][x].g;
			b = org_img[y][x].b;
			int difNum = 0;
			for (int j = -1; j<=1; ++j)
			{
				for (int k = -1; k<=1; ++k)
				{
					int iy = y + j;
					int ix = x + k;
					int rr = org_img[iy][ix].r;
					int gg = org_img[iy][ix].g;
					int bb = org_img[iy][ix].b;
			//		cout<<rr<<" "<<gg<<" "<<bb<<endl;
 					if (r != org_img[iy][ix].r || g != org_img[iy][ix].g || b != org_img[iy][ix].b)
 					{
 						difNum++;
 					}
				}
			//	cout<<endl;
			}
			
			if (difNum < 4)
			{
				cout<<"dif Number = "<<difNum<<endl;
				continue;
			}

			for (int j = -1; j<=1; ++j)
			{
				for (int k = -1; k<=1; ++k)
				{
					int iy = y + j;
					int ix = x + k;

					map<int, double> imap;
					double rightVal = 0.0;
					imap[unIdx*3]   = org_img[iy][ix].r;
					imap[unIdx*3+1] = org_img[iy][ix].g;
					imap[unIdx*3+2] = org_img[iy][ix].b;
					rightVal = sha_img[iy][ix];
					
					left_vec.push_back(imap);
					right_vec.push_back(rightVal);
				}				
			}
			map<int, double> imap;
			double rightVal = 0.0;
			imap[unIdx*3]   = epsilon2;
			imap[unIdx*3+1] = epsilon2;
			imap[unIdx*3+2] = epsilon2;
			rightVal = 0.0;

			left_vec.push_back(imap);
			right_vec.push_back(rightVal);

			unIdx++;
		}
	}

	if (left_vec.size() != right_vec.size())
	{
		return;
	}
	else
	{
		cout<<"complete equation"<<endl;
	}

	cout<<"left_vec size = "<<left_vec.size()<<endl;

	vector<double> result(unIdx * 3);
	cout<<"result size = "<<result.size()<<endl;
	//solve AX = b, b != 0
	bool isOk = LeastSquare::solver(left_vec, right_vec, true, result.size(), &result[0]);
	if (isOk)
	{
		Mat shadingResult = Mat(h, w, CV_8U);
		Bw8UImage sha_rst(&shadingResult);
		for (int y = 1, idx = 0; y<h-1; ++y)
		{
			for (int x = 1; x<w-1; ++x)
			{
				sha_rst[y][x] = result[3*idx] * org_img[y][x].r + 
								result[3*idx+1] * org_img[y][x].g +
								result[3*idx+2] * org_img[y][x].b;

			}
		}
		imshow("shading result", shadingResult);
	}

	//Si = a1*Ir + a2*Ig + a3*Ib + b
}

void test_4DlinearModelB(const Mat &orgImage, const Mat &shadingGT)
{
	//Si = a1*Ir + a2*Ig + a3*Ib + b
	vector<map<int, double>> left_vec;
	vector<double> right_vec;

	int w = orgImage.cols, h = orgImage.rows;
	Rgb8UImage org_img(&orgImage);
	Bw8UImage  sha_img(&shadingGT);

	//	const int WINDOW_SIZE = 2;
	double epsilon  = 0.0001;
	double epsilon2 = sqrt(epsilon); 
	int unIdx = 0;
	for (int y = 1; y<h-1; ++y)
	{
		for (int x = 1; x<w-1; ++x)
		{
			//check if all data is the same
			int r, g, b, s;
			r = org_img[y][x].r;
			g = org_img[y][x].g;
			b = org_img[y][x].b;
			int difNum = 0;
			for (int j = -1; j<=1; ++j)
			{
				for (int k = -1; k<=1; ++k)
				{
					int iy = y + j;
					int ix = x + k;
					if (r != org_img[iy][ix].r || g != org_img[iy][ix].g || b != org_img[iy][ix].b)
					{
						difNum++;
					}
				}
			}

			if (difNum < 4)
			{
				cout<<"dif Number = "<<difNum<<endl;
				continue;
			}

			for (int j = -1; j<=1; ++j)
			{
				for (int k = -1; k<=1; ++k)
				{
					int iy = y + j;
					int ix = x + k;

					map<int, double> imap;
					double rightVal = 0.0;
					imap[unIdx*4]   = org_img[iy][ix].r;
					imap[unIdx*4+1] = org_img[iy][ix].g;
					imap[unIdx*4+2] = org_img[iy][ix].b;
					imap[unIdx*4+3] = 1.0;
					rightVal = sha_img[iy][ix];

					left_vec.push_back(imap);
					right_vec.push_back(rightVal);
				}				
			}
			map<int, double> imap;
			double rightVal = 0.0;
			imap[unIdx*4]   = epsilon2;
			imap[unIdx*4+1] = epsilon2;
			imap[unIdx*4+2] = epsilon2;
			rightVal = 0.0;

			left_vec.push_back(imap);
			right_vec.push_back(rightVal);

			unIdx++;
		}
	}

	if (left_vec.size() != right_vec.size())
	{
		return;
	}
	else
	{
		cout<<"complete equation"<<endl;
	}

	cout<<"left_vec size = "<<left_vec.size()<<endl;

	vector<double> result(unIdx * 4);
	cout<<"result size = "<<result.size()<<endl;
	//solve AX = b, b != 0
	bool isOk = LeastSquare::solver(left_vec, right_vec, true, result.size(), &result[0]);
	if (isOk)
	{
		Mat shadingResult = Mat(h, w, CV_8U);
		Bw8UImage sha_rst(&shadingResult);
		for (int y = 1, idx = 0; y<h-1; ++y)
		{
			for (int x = 1; x<w-1; ++x)
			{
				sha_rst[y][x] = result[4*idx] * org_img[y][x].r + 
					result[4*idx+1] * org_img[y][x].g +
					result[4*idx+2] * org_img[y][x].b;

			}
		}
		imshow("B", shadingResult);
	}

	//Si = a1*Ir + a2*Ig + a3*Ib + b
}

void solve_intrinsic_ls_area(const Mat &orgImg, const Mat &shaAbsMap, 							 
							 const vector<vector<int>> &simRefPix, 
							 const vector<vector<int>> &simShaPix)
{
	//	cout<<orgImg.depth()<<endl;
	assert(orgImg.depth() == CV_8U);
	assert(orgImg.channels() == 3);
	const int w = orgImg.cols;
	const int h = orgImg.rows;

	//define constant parameters
	const int WSIZE = 3;    //size of the local windows
	const int WSIZE2 = 9;   //WSIZE2 = WSIZE*WSIZE, number of pixels in each window ('n' in the paper)
	const int MSIZE = 12;   //WSIZE2+3, number of rows in the M matrix (see equation 8)
	const int WSIZE_2 = 1;  //WSIZE_2 = WSIZE/2
	const int SIZE_N = 5;   //2*2*WSIZE_2+1, size of the total neighborhood affected by a pixel
	const int SIZE_N2 = 25; //SIZE_N*SIZE_N, number of non-zero element per-pixel in the L matrix 
	double epsilon = 0.000001;
	double sqrt_epsilon = sqrt(epsilon);

	
	//result Mat
	//unknown index map
	Mat rstMat   = Mat::zeros(h, w, CV_32F);
	Mat unIdxMat = Mat::zeros(h, w, CV_32S);

	Bw32SImage idx_map(&unIdxMat);
	Bw8UImage  abs_map(&shaAbsMap);
	Bw32FImage rst_mat(&rstMat);

	//set shading absolute area and unknown index area
	int unKnowNum = 0;
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			if (abs_map[y][x] != 0)
			{
				idx_map[y][x] = -1;
				rst_mat[y][x] = 1.0;
			}
			else
			{
				idx_map[y][x] = unKnowNum;
				unKnowNum++;
			}
		}
	}

	/* Set M */
	double M[MSIZE][3]; //M matrix (see equation 8)
	M[MSIZE-3][0] = sqrt_epsilon; M[MSIZE-3][1] = 0; M[MSIZE-3][2] = 0;
	M[MSIZE-2][0] = 0; M[MSIZE-2][1] = sqrt_epsilon; M[MSIZE-2][2] = 0;
	M[MSIZE-1][0] = 0; M[MSIZE-1][1] = 0; M[MSIZE-1][2] = sqrt_epsilon;

	vector<map<int, double>> matrix_vec;
	vector<double> right_vec;

	//equation 10 NiSi = 0
	Rgb8UImage org_img(&orgImg);
	for (int y = 1; y<h-1; ++y)
	{
		for (int x = 1; x<w-1; ++x)
		{
			//fill the M matrix with the image values in the current window
			//Equ 8
			for (int m=-WSIZE_2; m<=WSIZE_2; m++) 
			{
				int yC = y+m;
				for (int n=-WSIZE_2; n<=WSIZE_2; n++)
				{
					int xC = x+n;
					M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][0] = org_img[yC][xC].r;
					M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][1] = org_img[yC][xC].g;
					M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][2] = org_img[yC][xC].b;
				}
			}
			//get Ni
			Mat Mi  = Mat(MSIZE,3,CV_64F, M);
			Mat Mit = Mi.t();
			Mat MitMi = Mit * Mi;
			Mat MitMi_inv = MitMi.inv();
			Mat Ni = Mi * MitMi_inv * Mit;
			Ni = Mat::eye(Ni.rows, Ni.cols, CV_64F) - Ni;			

			double *pNi = (double*)(Ni.data);
			for (int j = 0; j<Ni.rows; ++j)
			{
				map<int, double> imap;
				double rightVal = 0.0;
				for (int k = 0; k<Ni.cols-3; ++k)
				{
					int niIdx = j * Ni.cols + k;

					//sIdx position in original map
					int sy = k/WSIZE - WSIZE_2 + y;
					int sx = k%WSIZE - WSIZE_2 + x;
					//shading value already known
					if (idx_map[sy][sx] == -1)
					{
						rightVal -= pNi[niIdx] * rst_mat[sy][sx];
					}
					else
					{						
						int sIdx = idx_map[sy][sx];
						imap[sIdx] = pNi[niIdx];	
					}

				}
				matrix_vec.push_back(imap);
				right_vec.push_back(rightVal);
			}
		}
	}

	//Equ 13 constant reflectance
	for (int i = 0; i<simRefPix.size(); ++i)
	{
		for (int j = 0; j<simRefPix[i].size(); ++j)
		{
			int idx1 = simRefPix[i][j];
			float wt = 1.0/(simRefPix[i].size() * simRefPix[i].size()/3.0);
			for (int k = 0; k<simRefPix[i].size(); ++k)
			{
				if (k != j)
				{
					int idx2 = simRefPix[i][k];

					int x1 = idx1%w, y1 = idx1/w;
					int x2 = idx2%w, y2 = idx2/w;

					int unIdx1 = idx_map[y1][x1];
					int unIdx2 = idx_map[y2][x2];

					if (unIdx1 != -1 && unIdx2 != -1)
					{
						map<int, double> imap;
						double rightVal = 0.0;

						imap[unIdx1]  =  wt * org_img[y2][x2].r;
						imap[unIdx2]  = -wt * org_img[y1][x1].r;
						matrix_vec.push_back(imap);
						right_vec.push_back(rightVal);
						imap.clear();

						imap[unIdx1] =   wt * org_img[y2][x2].g;
						imap[unIdx2]  = -wt * org_img[y1][x1].g;
						matrix_vec.push_back(imap);
						right_vec.push_back(rightVal);
						imap.clear();

						imap[unIdx1] =  wt * org_img[y2][x2].b;
						imap[unIdx2]  = -wt * org_img[y1][x1].b;
						matrix_vec.push_back(imap);
						right_vec.push_back(rightVal);
						imap.clear();
					}					
				}
			}
		}		
	}
	
	//Equ 15
	for (int i = 0; i<simShaPix.size(); ++i)
	{
		for (int j = 0; j<simShaPix[i].size(); ++j)
		{
			int idx1 = simShaPix[i][j];
			float wt = 1.0/(simShaPix[i].size() * simShaPix[i].size()/3.0);
			for (int k = 0; k<simShaPix[i].size(); ++k)
			{
				if (k != j)
				{
					int idx2 = simShaPix[i][k];

					int x1 = idx1%w, y1 = idx1/w;
					int x2 = idx2%w, y2 = idx2/w;

					int unIdx1 = idx_map[y1][x1];
					int unIdx2 = idx_map[y2][x2];

					if (unIdx1 != -1 && unIdx2 != -1)
					{
						map<int, double> imap;
						double rightVal = 0.0;


						imap[unIdx1]  =  wt * 1.0;
						imap[unIdx2]  = -wt * 1.0;
						matrix_vec.push_back(imap);
						right_vec.push_back(rightVal);						
					}					
				}
			}
		}		
	}

	if (matrix_vec.size() != right_vec.size())
	{
		return;
	}
	else
	{
		cout<<"complete equation"<<endl;
	}

	cout<<"matris_vec size = "<<matrix_vec.size()<<endl;

	vector<double> result(unKnowNum);
	//solve AX = b, b != 0
	//	bool isOk = CvxSF::LeastSquare(matrix_vec, right_vec, result);
	bool isOk = LeastSquare::solver(matrix_vec, right_vec, true, unKnowNum, &result[0]);
	if (isOk)
	{
		for (int y = 0; y<h; ++y)
		{
			for (int x = 0; x<w; ++x)
			{
				if (idx_map[y][x] != -1)
				{
					rst_mat[y][x] = result[idx_map[y][x]];					
				}				
			}
		}

		Mat reflectanceMat(rstMat.rows, rstMat.cols, CV_8UC3);

		Rgb8UImage ref_mat(&reflectanceMat);
		for (int y = 0; y<h; ++y)
		{
			for (int x =0 ; x<w; ++x)
			{
				float sVal = rst_mat[y][x];
				if (sVal != 0.0)
				{
					ref_mat[y][x].r = org_img[y][x].r /sVal;
					ref_mat[y][x].g = org_img[y][x].g /sVal;
					ref_mat[y][x].b = org_img[y][x].b /sVal;
				}
			}
		}	

		imshow("reflectance image", reflectanceMat);
		imwrite("flower_ref.png", reflectanceMat);


		rstMat.convertTo(rstMat, CV_8U, 255.0);
		imshow("result", rstMat);
		imwrite("flower_LS.png", rstMat);


	}
	else
	{
		cout<<"least square failed"<<endl;
	}

}